﻿using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
//using System.Linq;
using System.Text;
using System.Windows.Forms;

namespace ekf_slam_simulator
{
    
    public partial class MainForm : Form
    {
        Graphics g;
        List<Point> follow = new List<Point>();
        Point next;
        Point roboLoc = new Point(250, 50);
        Point ghostLoc = new Point(250, 50);
        double roboAng = 0d;
        double GhostAng = 0d;
        Space space = new Space();                //New space created.
        LinkedList<Point> landmark_tmp = new LinkedList<Point>();
        LinkedList<Point> landmark_tmp2 = new LinkedList<Point>();
        bool timer_ticking = true;
        String infoTxt = "Go!";
        Color infoColor = Color.Green;
        bool ctrl = false;
        bool helpOn = false;
        int id_iterator = 0;

        public MainForm()
        {
            this.TopMost = true;                  //Sets frame always over console.
            InitializeComponent();
            this.DoubleBuffered = true;
            SetStyle(ControlStyles.AllPaintingInWmPaint, true);
            SetStyle(ControlStyles.OptimizedDoubleBuffer, true);
            UpdateStyles();
            
            space.robot.setLoc(roboLoc);          //Sets initial robot position.
            space.robot.setLocEst(roboLoc);
            next = roboLoc;
            //follow.Add(next);

             
            Timer.Start();

        }

        private void MainForm_Load(object sender, EventArgs e)
        {

        }

        /////////////////////////////////////////////////////////////////////////////
        private void MainForm_Paint(object sender, PaintEventArgs e)
        {
            int rozm = 50;
            g = e.Graphics;
            //g.FillRectangle(new SolidBrush(Color.Aquamarine), 10, 10, 10,10);
            for (int i = 0; i <= 16; i++)
            {
                g.DrawLine(new Pen(Color.BlueViolet), i * rozm, 0, i * rozm, 600);
            }
            for (int i = 0; i <= 12; i++)
            {
                g.DrawLine(new Pen(Color.BlueViolet), 0, i * rozm, 800, i * rozm);
            }

            //objects
            foreach (Landmark l in space.landmark_real)
                 drawObject(l.loc.X, l.loc.Y);

            //ghost robot
            Color transGrey = Color.FromArgb(100, 100, 100, 100);
            drawRobot(ghostLoc.X, ghostLoc.Y, GhostAng, transGrey);

            //robot
            drawRobot(roboLoc, roboAng, Color.Black);

            
            //lasers
            foreach(Point p in landmark_tmp2)
                drawLaser(roboLoc, p);
            
            //path
            drawPath();

            //text
            g.FillRectangle(new SolidBrush(Color.White), 0, 600, 1000, 50);
            g.DrawRectangle(new Pen(Color.Black), 0, 600, 1000, 50);
            g.FillRectangle(new SolidBrush(Color.White), 800, 0, 200, 600);
            g.DrawRectangle(new Pen(Color.Black), 800, 0, 200, 600);

            drawCenterText(infoTxt, infoColor, 900, 10, 28);
            drawCenterText("press Space to pause/resume", Color.Black, 900, 50, 12);

            Color c;
            if (120 - Timer.Interval < 40) c = Color.Blue;
            else if (120 - Timer.Interval < 90) c = Color.Green;
            else c = Color.Red;
            drawCenterText("PgDn", Color.Black, 830, 190, 12);
            drawCenterText("Speed: " + (120 - Timer.Interval), c, 900, 190, 12);
            drawCenterText("PgUp", Color.Black, 970, 190, 12);

            drawCenterText("Sensor Noises", Color.Black, 900, 420, 15);
            drawCenterText("q", Color.Black, 830, 450, 12);
            drawCenterText("Lidar: " + space.robot.lidNR, Color.Green, 900, 450, 12);
            drawCenterText("w", Color.Black, 970, 450, 12);
            drawCenterText("a", Color.Black, 830, 470, 12);
            drawCenterText("Odometer: " + space.robot.odoNR, Color.Green, 900, 470, 12);
            drawCenterText("s", Color.Black, 970, 470, 12);

            String s;
            if (helpOn) s = "Turn Help Off";
            else s = "Turn Help On";
            drawCenterText(s, Color.Black, 900, 560, 15);

            if (space.ekfOn) drawCenterText("Turn EKF Off", Color.Blue, 900, 520, 15);
            else drawCenterText("Turn EKF On", Color.Red, 900, 520, 15);

            //help
            drawText("Keyboard", Color.Blue, 10, 605, 25);
            drawText("press c to clear, PgUp/PgDn changes Speed", Color.Black, 170, 600, 12);
            drawText("press q/w to decrease/increase Lidar Noise", Color.Black, 170, 615, 12);
            drawText("press a/s to decrease/increase Odometer Noise", Color.Black, 170, 630, 12);
            drawText("Mouse", Color.Blue, 500, 605, 25);
            drawText("click Right to place Orientation Point (+Ctrl to remove)", Color.Black, 620, 600, 12);
            drawText("click Left to place Track Point (+Ctrl to remove)", Color.Black, 620, 615, 12);
            drawText("click Wheel to remove any Point", Color.Black, 620, 630, 12);

            //speedmeter
            drawSpeedmeter(120-Timer.Interval);

            //map
            drawMap();

            
        }

        //..........
        private void drawRobot(int x, int y, double fi, Color color)
        {
            int r = 30;
            fi = -fi*Math.PI/180;

            Point[] roboShape = {
                new Point( x+(int)(r*Math.Cos(fi)), y+(int)(r*Math.Sin(fi)) ),
                new Point( x+(int)(r*Math.Cos(fi+Math.PI/4)), y+(int)(r*Math.Sin(fi+Math.PI/4)) ),
                new Point( x+(int)(r*Math.Cos(fi+Math.PI*3/4)), y+(int)(r*Math.Sin(fi+Math.PI*3/4)) ),                
                new Point( x+(int)(r*Math.Cos(fi+Math.PI*5/4)), y+(int)(r*Math.Sin(fi+Math.PI*5/4)) ),
                new Point( x+(int)(r*Math.Cos(fi+Math.PI*7/4)), y+(int)(r*Math.Sin(fi+Math.PI*7/4)) ),           
                new Point( x+(int)(r*Math.Cos(fi)), y+(int)(r*Math.Sin(fi)) )                  
            };

            //g.DrawLine(new Pen(Color.Black), x, y, x+(int)(r*Math.Cos(fi)), y+(int)(r*Math.Sin(fi)));
            //g.DrawPolygon(new Pen(Color.Red), roboShape);
            g.FillPolygon(new SolidBrush(color), roboShape);
        }
        private void drawRobot(Point p, double fi, Color color)
        {
            drawRobot(p.X, p.Y, fi, color);
        }

        //..........
        private void drawObject(int x, int y)
        {
            int r = 14;
            int l = 2;
            g.DrawEllipse(new Pen(Color.Green), x-r/2, y-r/2, r, r);
            g.FillEllipse(new SolidBrush(Color.Green), x - r / 2 + l, y - r / 2 + l, r - 2 * l, r - 2 * l);
        }
        private void drawObject(Point p)
        {
            drawObject(p.X, p.Y);
        }

        //..........
        private void drawLaser(int x1, int y1, int x2, int y2)
        {
             g.DrawLine(new Pen(Color.Red), x1, y1, x2, y2);
        }
        private void drawLaser(Point p1, Point p2)
        {
            drawLaser(p1.X, p1.Y, p2.X, p2.Y);
        }

        //..........
        private void drawPath()
        {
            int r = 2;
            Point prev = new Point(roboLoc.X, roboLoc.Y);
            foreach (Point p in follow)
            {
                g.DrawRectangle(new Pen(Color.DarkOrange), p.X - r, p.Y - r, 2 * r, 2 * r);
                g.DrawLine(new Pen(Color.DarkOrange), prev.X, prev.Y, p.X, p.Y);
                prev = p;
            }
        }
        
        //..........
        private void drawText(String str)
        {
             drawText(str, Color.Black);
        }

        private void drawText(String str, Color c)
        {
             drawText(str, c, 5, 400, 20);
        }

        private void drawText(String str, Color c, int x, int y, int size)
        {
             Font f = new Font(new FontFamily("Times New Roman"), size);
             g.DrawString(str, f, new SolidBrush(c), x, y);
        }
        private void drawCenterText(String str, Color c, int x, int y, int size)
        {
            StringFormat drawFormat = new StringFormat();
            drawFormat.Alignment = StringAlignment.Center;
            Font f = new Font(new FontFamily("Times New Roman"), size);
            g.DrawString(str, f, new SolidBrush(c), x, y, drawFormat);
        }
        

        //..........
        private void drawSpeedmeter(int a)
        {
            int offX = 820, offY = 100;
            int r = 80, indicator = 70; 
            int maxV = 120;
            double phi = a * 180 / maxV;
            phi = Math.PI + Math.PI * phi / 180;
            g.DrawArc(new Pen(Color.Black), offX, offY, 2*r, 2*r, 180, 180);
            g.DrawLine(new Pen(Color.Black), offX, offY+r, offX+2*r, offY+r);
            g.DrawLine(new Pen(Color.Red,4), offX + r, offY + r, (float)(offX + r + indicator * Math.Cos(phi)), (float)(offY + r + indicator * Math.Sin(phi)));
        }

        //..........
        private void drawMap()
        {
            int offX = 820, offY = 250;       
            g.FillRectangle(new SolidBrush(Color.Gray), offX, offY, 160, 120);
            
            //net
            for (int i = 1; i < 16; i++) g.DrawLine(new Pen(Color.DarkGray), offX + 10 * i, offY, offX + 10 * i, offY + 120);
            for (int i = 1; i < 12; i++) g.DrawLine(new Pen(Color.DarkGray), offX, offY + 10 * i, offX+160, offY + 10 * i);

            g.DrawRectangle(new Pen(Color.Black), offX, offY, 160, 120);

            foreach (Landmark l in space.robot.landmark_est)
                drawMapObject(offX + l.loc.X / 5, offY + l.loc.Y / 5);
            drawMapRobot(offX + ghostLoc.X / 5, offY + ghostLoc.Y / 5, GhostAng, Color.White);
        }
        private void drawMapObject(int x, int y)
        {
            int r = 7;
            g.FillEllipse(new SolidBrush(Color.Red), x - r / 2, y - r / 2, r , r);
        }
        private void drawMapRobot(int x, int y, double fi, Color color)
        {
            int r = 9;
            fi = -fi * Math.PI / 180;

            Point[] roboShape = {
                new Point( x+(int)(r*Math.Cos(fi)), y+(int)(r*Math.Sin(fi)) ),
                new Point( x+(int)(r*Math.Cos(fi+Math.PI/4)), y+(int)(r*Math.Sin(fi+Math.PI/4)) ),
                new Point( x+(int)(r*Math.Cos(fi+Math.PI*3/4)), y+(int)(r*Math.Sin(fi+Math.PI*3/4)) ),                
                new Point( x+(int)(r*Math.Cos(fi+Math.PI*5/4)), y+(int)(r*Math.Sin(fi+Math.PI*5/4)) ),
                new Point( x+(int)(r*Math.Cos(fi+Math.PI*7/4)), y+(int)(r*Math.Sin(fi+Math.PI*7/4)) ),           
                new Point( x+(int)(r*Math.Cos(fi)), y+(int)(r*Math.Sin(fi)) )                  
            };

            g.FillPolygon(new SolidBrush(color), roboShape);
        }
        ////////////////////////////////////////////////////////////////////////////

        private void setText(String txt)
        {
             infoTxt = txt;
             infoColor = Color.Black;
        }

        private void setText(String txt, Color c)
        {
             infoTxt = txt;
             infoColor = c;
        }

 

        private void Timer_Tick(object sender, EventArgs e)
        {
             if (timer_ticking)                                         //Flag for pausing action with space key.
             {
                 if (follow.Count == 0)                                 //If follow list is empty, stop the robot.
                 {
                     next = space.robot.location_real;
                     //follow.Add(next);
                 }
                 else if (follow[0] != next)                            //If robot goes to deleted goal change it's course.
                     next = follow[0];

                 else if (space.robot.follow(next))                     //If robot reached destination,remove current goal from list.
                 {     
                     follow.RemoveAt(0);
                     if(follow.Count>0)                                 //If there are further goals then realize it.
                        next = follow[0];

                 }
                              
                  landmark_tmp = space.robot.observe();                 //Makes robot look for landmarks.
                  landmark_tmp2 = space.robot.observeLaser();
                  roboLoc = space.robot.location_real;
                  roboAng = space.robot.angle_real;
                  GhostAng = space.robot.angle_est;
                  ghostLoc = space.robot.location_est;

             }
             Refresh();
        }

        ////////////////////////////////////////////////////////////////////////////
        private void MainForm_MouseClick(object sender, MouseEventArgs e)
        {
            if(e.X<800 && e.Y<600)
                switch (e.Button)
                {
                     case MouseButtons.Left:
                          if (ctrl == false)
                               follow.Add(e.Location);
                          else
                               removeFollowPoint(e.Location);
                          break;
                     case MouseButtons.Right:
                          if (ctrl == false)
                          {
                               Landmark l;
                               l.loc = e.Location;
                               l.id = id_iterator;
                               id_iterator++;
                               space.landmark_real.Add(l);
                          }
                          else
                               removeLandmarkPoint(e.Location);
                          break;
                     case MouseButtons.Middle:
                          removePoint(e.Location);
                          break;

                }
            if (((e.X > 800 && e.Y > 550) || e.Y > 600) && e.Button == MouseButtons.Left)
            {
                if (helpOn == true)
                {
                    this.SetBounds(0, 0, 0, 629, BoundsSpecified.Height);
                    helpOn = false;
                }
                else
                {
                    this.SetBounds(0, 0, 0, 679, BoundsSpecified.Height);
                    helpOn = true;
                }
            }
            if (e.X > 800 && e.Y < 550 && e.Y > 500)
            {
                if (space.ekfOn == true)
                {
                    space.ekfOn = false;
                }
                else
                {
                    space.ekfOn = true;
                }
                space.robot.resetData();
            }
        }

        //..........
        private void removePoint(Point e)
        {
             int field = 5;
             for (int i = e.X - field; i < e.X + field; i++)
                  for (int j = e.Y - field; j < e.Y + field; j++)
                  {
                       Landmark k;
                       k.id = -1;
                       k.loc = new Point();
                       follow.Remove(new Point(i, j));
                       Point p = new Point(i, j);
                       foreach (Landmark l in space.landmark_real)
                            if (l.loc == p)
                                 k = l;
                       if (k.id != -1)
                            space.landmark_real.Remove(k);
                  }
        }

        //..........
        private void removeLandmarkPoint(Point e)
        {
             int field = 5;
             for (int i = e.X - field; i < e.X + field; i++)
                  for (int j = e.Y - field; j < e.Y + field; j++)
                  {
                       Landmark k;
                       k.id = -1;
                       k.loc = new Point();
                       Point p = new Point(i, j);
                       foreach (Landmark l in space.landmark_real)
                            if (l.loc == p)
                                 k = l;
                       if (k.id != -1)
                            space.landmark_real.Remove(k);
                  }
        }

        //..........
        private void removeFollowPoint(Point e)
        {
            int field = 5;
            for (int i = e.X - field; i < e.X + field; i++)
                for (int j = e.Y - field; j < e.Y + field; j++)
                {
                    follow.Remove(new Point(i, j));
                }
        }

        ////////////////////////////////////////////////////////////////////////////
        private void MainForm_KeyDown(object sender, KeyEventArgs e)
        {
             Refresh();
             switch (e.KeyCode)
             {
                  case Keys.Space:
                       timer_ticking = !timer_ticking;
                       if (timer_ticking)
                            setText("Go!", Color.Green);
                       else
                            setText("Pause", Color.Red);
                       break;
                  case Keys.C:
                       follow.Clear();
                       //follow.Add(space.robot.location_real);
                       next = space.robot.location_real;
                       space.landmark_real.Clear();
                       break;
                  case Keys.PageUp:
                       if(Timer.Interval > 1)
                            Timer.Interval--;
                       break;
                  case Keys.PageDown:
                       if(Timer.Interval < 119)
                            Timer.Interval++;
                       break;

                  case Keys.Q:
                       if (space.robot.lidNR > 0)
                           space.robot.lidNR--;
                       break;
                  case Keys.W:
                       if (space.robot.lidNR < 10)
                           space.robot.lidNR++;
                       break;
                  case Keys.A:
                       if (space.robot.odoNR > 0)
                           space.robot.odoNR--;
                       break;
                  case Keys.S:
                       if (space.robot.odoNR < 10)
                           space.robot.odoNR++;
                       break;

                 case Keys.ControlKey:
                       ctrl = true;
                       break;
                 case Keys.P:
                       space.robot.updateX();
                       break;

                  default:
                       break;
             }
        }

        //..........
        private void MainForm_KeyUp(object sender, KeyEventArgs e)
        {
            switch (e.KeyCode)
            {
                case Keys.ControlKey:
                    ctrl = false;
                    break;
            }
        }
        ////////////////////////////////////////////////////////////////////////////
    }
}
